﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Data;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Numerics;
using MathNet.Numerics;
using System.Collections.Specialized;
namespace QuadRemote.Tabs
{
    public partial class BodePlotter : UserControl
    {
        private float fromLog = 0.1f, toLog = 100f;
        int offsetLeft = 20, offsetTop = 20, offsetRight = 20, offsetBottom = 20;
        Boolean redrawGraphs = true;

        int displayType = 0;
        BodeList liveBode;
        List<BodeList> imported;

        public BodePlotter()
        {
            InitializeComponent();
            liveBode = new BodeList();
            imported = new List<BodeList>();
            
            this.DoubleBuffered = true;
            this.BackColor = Color.Black;
            this.MouseWheel += BodePlotter_MouseWheel;

            refreshFiles();

            StringCollection loaded = Properties.Settings.Default.ESCBodeLoaded;

            if (loaded != null)
                foreach (String s in loaded)
                    importFile(s);

            StringCollection live = Properties.Settings.Default.ESCBodeLive;

            if (live != null)
                foreach (String s in live)
                    liveBode.addLine(s);
        }

        void BodePlotter_MouseWheel(object sender, MouseEventArgs e)
        {
            if (MX < offsetLeft || MX > Width - offsetRight) return;

            float zoom = 0.2f;

            float x = (MX - offsetLeft) / (float)(Width - offsetLeft - offsetRight) * zoom;
            if (e.Delta < 0)
            {
                fromLog = fromLog * (1f + x);
                toLog = toLog * (1f - zoom + x);
            }
            else if (e.Delta > 0)
            {
                fromLog = fromLog * (1f - x);
                toLog = toLog * (1f + zoom - x);
            }

            if (fromLog < 0.1) fromLog = 0.1f;
            if (toLog > 100) toLog = 100;
            if (toLog < 1) toLog = 1;
            if (fromLog > 10) fromLog = 10f;
            if (fromLog == toLog) toLog += 1f;

            redrawGraphs = true;
            Invalidate();
        }

        protected override void OnPaint(PaintEventArgs e)
        {
            base.OnPaint(e);

            long debugRender = DateTime.Now.Ticks;

            Graphics g = e.Graphics;
            StringFormat centerFormat = new StringFormat();
            centerFormat.Alignment = StringAlignment.Center;
            centerFormat.LineAlignment = StringAlignment.Center;
            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;

            Color[] colors = new Color[] { Color.Red, Color.Yellow, Color.Cyan, Color.Lime, Color.Violet, Color.Orange, Color.Magenta};
            
            float logWidth = (Width - offsetLeft - offsetRight) / (float)((Math.Log10(toLog)+1)-(Math.Log10(fromLog)+1));

            float val = fromLog;
            float valOffset = (float)(Math.Log10(val) + 1) * logWidth;
            while (val <= toLog)
            {
                float x = (float)(Math.Log10(val) + 1) * logWidth - valOffset;

                g.DrawLine(Pens.Gray, x + offsetLeft, offsetTop, x + offsetLeft, Height / 2 - offsetBottom);
                g.DrawLine(Pens.Gray, x + offsetLeft, offsetTop + Height / 2, x + offsetLeft, Height - offsetBottom);

                if (x - 20 > MX - offsetLeft || x + 20 < MX - offsetLeft)
                    g.DrawString(Math.Round(val,1) + "", Tools.getPrimaryFont(), Brushes.White, x + offsetLeft, Height / 2, centerFormat);

                val += (float)Math.Pow(10, Math.Floor(Math.Log10(val)));
            }

            float gainMin = -2;
            float gainMax = 2;
            float phaseMin = -20;
            float phaseMax = 20;

            if (liveBode.redraw || redrawGraphs)
            {
                liveBode.minGain = liveBode.getMin(displayType, gainMin, fromLog, toLog);
                liveBode.maxGain = liveBode.getMax(displayType, gainMax, fromLog, toLog);
                liveBode.minPhase = liveBode.getMin(displayType+3, phaseMin, fromLog, toLog);
                liveBode.maxPhase = liveBode.getMax(displayType+3, phaseMax, fromLog, toLog);
            }
            gainMin = liveBode.minGain < gainMin ? liveBode.minGain : gainMin;
            gainMax = liveBode.maxGain > gainMax ? liveBode.maxGain : gainMax;
            phaseMin = liveBode.minPhase < phaseMin ? liveBode.minPhase : phaseMin;
            phaseMax = liveBode.maxPhase > phaseMax ? liveBode.maxPhase : phaseMax;

            foreach (BodeList import in imported)
            {
                if (import.redraw || redrawGraphs)
                {
                    import.minGain = import.getMin(displayType, gainMin, fromLog, toLog);
                    import.maxGain = import.getMax(displayType, gainMax, fromLog, toLog);
                    import.minPhase = import.getMin(displayType+3, phaseMin, fromLog, toLog);
                    import.maxPhase = import.getMax(displayType+3, phaseMax, fromLog, toLog);
                }
                gainMin = import.minGain < gainMin ? import.minGain : gainMin;
                gainMax = import.maxGain > gainMax ? import.maxGain : gainMax;
                phaseMin = import.minPhase < phaseMin ? import.minPhase : phaseMin;
                phaseMax = import.maxPhase > phaseMax ? import.maxPhase : phaseMax;
            }

            float gainRange = gainMax - gainMin;
            float phaseRange = phaseMax - phaseMin;

            float gainScale = gainRange / (Height / 2 - offsetBottom - offsetTop);
            float phaseScale = phaseRange / (Height / 2 - offsetBottom - offsetTop);

            float gainZero = gainMax / gainScale;
            float phaseZero = phaseMax / phaseScale;

            float startGain = gainZero * gainScale;
            float YInc = gainScale < 0.05 ? 1 : (gainScale < 0.1 ? 3 : 9);
            startGain = (float)Math.Round(startGain / YInc, 0) * YInc;
            float YPos = -startGain / gainScale + gainZero + offsetTop;

            while (YPos < Height / 2 - offsetBottom)
            {
                if (YPos >= offsetTop && YPos <= Height / 2 - offsetBottom)
                {
                    g.DrawLine(Pens.Gray, offsetLeft, YPos, Width - offsetRight, YPos);
                    g.DrawString(startGain + "dB", Tools.getPrimaryFont(), Brushes.White, offsetLeft - 15, YPos - 5);
                }
                startGain -= YInc;
                YPos = -startGain / gainScale + gainZero + offsetTop;
            }

            /*
            for (int y = -12; y < 13; y += 3)
            {
                int pos = (int)(-y / gainScale + gainZero + offsetTop);
                if (pos < offsetTop || pos > Height / 2 - offsetBottom) continue;
                g.DrawLine(Pens.Gray, offsetLeft, pos, Width - offsetRight, pos);
                g.DrawString(y + "dB", Tools.getPrimaryFont(), Brushes.White, offsetLeft-15, pos-5);
            }*/

            float startPhase = phaseZero * phaseScale;
            YInc = phaseScale < 0.5 ? 15 : (phaseScale < 1 ? 45 : 90);
            startPhase = (float)Math.Round(startPhase / YInc, 0) * YInc;
            YPos = -startPhase / phaseScale + phaseZero + offsetTop + Height / 2;

            while (YPos < Height - offsetBottom)
            {
                if (YPos >= offsetTop + Height / 2 && YPos <= Height - offsetBottom)
                {
                    g.DrawLine(Pens.Gray, offsetLeft, YPos, Width - offsetRight, YPos);
                    g.DrawString(startPhase + "deg", Tools.getPrimaryFont(), Brushes.White, offsetLeft, YPos);
                }
                startPhase -= YInc;
                YPos = -startPhase / phaseScale + phaseZero + offsetTop + Height / 2;
            }

            /*
            for (int y = -180; y <= 180; y += 45)
            {
                int pos = (int)(-y / phaseScale + phaseZero + offsetTop + Height / 2);
                if (pos < offsetTop + Height / 2 || pos > Height - offsetBottom) continue;
                g.DrawLine(Pens.Gray, offsetLeft, pos, Width - offsetRight, pos);
                g.DrawString(y + "deg", Tools.getPrimaryFont(), Brushes.White, offsetLeft, pos);
            }*/

            float GX = 0;
            if (MX > offsetLeft && MX < Width - offsetRight)
            {
                Pen p = new Pen(Color.FromArgb(64,Color.White));
                g.DrawLine(p, MX, offsetTop, MX, Height / 2 - offsetBottom);
                g.DrawLine(p, MX, offsetTop + Height / 2, MX, Height - offsetBottom);
                GX = MX - offsetLeft;
                GX /= logWidth;
                GX = (float)Math.Pow(10,GX+Math.Log10(fromLog));
                g.DrawString(Math.Round(GX, 1) + "", Tools.getPrimaryFont(), Brushes.White, MX, Height / 2, centerFormat);
            }

            int c = 0;
            if (liveBode.Count > 1)
                imported.Insert(0, liveBode);
            else
                c++;
            foreach (BodeList plot in imported)
            {
                if (plot.redraw || redrawGraphs)
                {
                    plot.gainGraph = new PointF[plot.Count];
                    plot.phaseGraph = new PointF[plot.Count];

                    int i = 0;
                    foreach (BodeMeasurement b in plot)
                    {
                        plot.gainGraph[i].Y = b.getValue(displayType);
                        plot.phaseGraph[i].Y = b.getValue(displayType+3);

                        plot.gainGraph[i].X = (float)(Math.Log10(b.freq) - Math.Log10(fromLog)) * logWidth;
                        plot.phaseGraph[i].X = (plot.gainGraph[i].X);

                        i++;
                    }
                    plot.redraw = false;
                }

                g.SetClip(new Rectangle(offsetLeft, offsetTop, Width - offsetLeft - offsetRight, Height / 2 - offsetTop - offsetBottom));
                g.TranslateTransform(offsetLeft, gainZero + offsetTop);
                g.ScaleTransform(1, -1 / gainScale);

                if (plot.Count > 1) g.DrawLines(new Pen(colors[c], 1), plot.gainGraph);
                g.ResetTransform();

                g.SetClip(new Rectangle(offsetLeft, offsetTop + Height / 2, Width - offsetLeft - offsetRight, Height / 2 - offsetTop - offsetBottom));
                g.TranslateTransform(offsetLeft, phaseZero + offsetTop + Height / 2);
                g.ScaleTransform(1, -1 / phaseScale);

                if (plot.Count > 1) g.DrawLines(new Pen(colors[c], 1), plot.phaseGraph);
                g.ResetTransform();
                g.SetClip(this.ClientRectangle);

                String addValue = "";
                int radius = 6;
                if (MX > offsetLeft && MX < Width - offsetRight)
                {
                    for (int i = 1; i < plot.gainGraph.Length; i++)
                    {
                        if (plot.gainGraph[i].X >= MX - offsetLeft)
                        {
                            float dyG = (plot.gainGraph[i].Y - plot.gainGraph[i - 1].Y) / (plot.gainGraph[i].X - plot.gainGraph[i - 1].X);
                            float yyG = plot.gainGraph[i - 1].Y - dyG * (plot.gainGraph[i - 1].X - MX + offsetLeft);
                            g.DrawEllipse(new Pen(colors[c]), MX - radius / 2, yyG / -gainScale + offsetTop - radius / 2 + gainZero, radius, radius);

                            g.DrawString(Math.Round(yyG, 1) + "dB", Tools.getPrimaryFont(), new SolidBrush(colors[c]), MX, yyG / -gainScale + offsetTop + gainZero - 20, centerFormat);

                            float dyP = (plot.phaseGraph[i].Y - plot.phaseGraph[i - 1].Y) / (plot.phaseGraph[i].X - plot.phaseGraph[i - 1].X);
                            float yyP = plot.phaseGraph[i - 1].Y - dyP * (plot.phaseGraph[i - 1].X - MX + offsetLeft);
                            g.DrawEllipse(new Pen(colors[c]), MX - radius / 2, yyP / -phaseScale + offsetTop - radius / 2 + phaseZero + Height / 2, radius, radius);

                            g.DrawString(Math.Round(yyP, 1) + "deg", Tools.getPrimaryFont(), new SolidBrush(colors[c]), MX, yyP / -phaseScale + offsetTop + phaseZero + Height / 2 - 20, centerFormat);
                            //addValue = ": " + Math.Round(yyG, 1) + "dB " + Math.Round(yyP,1) + "deg";
                            break;
                        }
                    }

                }

                // frequency where closeloop gain crosses -3db
                float bandwidth = plot.getFreqForCross(1, -3);

                // gain at freq where openloop phase goes through -180
                float gainMarginFreq = plot.getFreqForCross(3, -180);
                float gainMargin = -plot.getValueForFreq(0, gainMarginFreq);

                // phase at freq where openloop phase goes through 0db
                float phaseMarginFreq = plot.getFreqForCross(0, 0);
                float phaseMarginExact = plot.getValueForFreq(3, phaseMarginFreq);
                float phaseMargin = 180 - phaseMarginExact;
                while (phaseMargin > 180) phaseMargin -= 360;
                while (phaseMargin < -180) phaseMargin += 360;
                if (phaseMargin < 0) phaseMargin = -phaseMargin;

                //float gain0db = plot.getFreqForCross(displayType, 0);
                //Console.WriteLine(gain0db + " gain through 0 ==> " + plot.getValueForFreq(displayType,gain0db));
                
                if (displayType == 0)
                {
                    Pen p = new Pen(Color.FromArgb(64, colors[c]), 1);
                    float x = (float)(Math.Log10(gainMarginFreq) + 1) * logWidth - (float)(Math.Log10(fromLog) + 1) * logWidth;
                    g.DrawLine(p, x + offsetLeft, offsetTop, x + offsetLeft, Height / 2 - offsetBottom);
                    g.DrawLine(p, x + offsetLeft, offsetTop + Height / 2, x + offsetLeft, Height - offsetBottom);
                    g.DrawLine(new Pen(colors[c], 2), x + offsetLeft, offsetTop + gainZero, x + offsetLeft, gainMargin / gainScale + offsetTop + gainZero);

                    x = (float)(Math.Log10(phaseMarginFreq) + 1) * logWidth - (float)(Math.Log10(fromLog) + 1) * logWidth;
                    g.DrawLine(p, x + offsetLeft, offsetTop + Height / 2, x + offsetLeft, Height - offsetBottom);
                    g.DrawLine(p, x + offsetLeft, offsetTop, x + offsetLeft, Height /2 - offsetBottom);
                    g.DrawLine(new Pen(colors[c], 2), x + offsetLeft, Height / 2 + offsetTop + phaseZero + 180 / phaseScale, x + offsetLeft, phaseMarginExact / -phaseScale + offsetTop + Height / 2 + phaseZero);
                }
                else if (displayType == 1)
                {
                    float x = (float)(Math.Log10(bandwidth) + 1) * logWidth - (float)(Math.Log10(fromLog) + 1) * logWidth;
                    g.DrawLine(new Pen(colors[c],2), x + offsetLeft - 5, offsetTop + gainZero + 3 / gainScale, x + offsetLeft + 5, offsetTop + gainZero + 3 / gainScale);
                }




                g.DrawString(plot.name, Tools.getPrimaryFont(), new SolidBrush(colors[c]), c * 200 + 5, 2);
                g.DrawString(Math.Round(bandwidth,1) + "hz bw, " + Math.Round(gainMargin,1) + "dB, " + Math.Round(phaseMargin,1) + " deg", Tools.getPrimaryFont(), new SolidBrush(colors[c]), c * 200 + 5, 2+15);

                c++;
                if (c >= colors.Length) c = 0;
            }

            if (liveBode.Count > 1)
                imported.RemoveAt(0);

            //Console.WriteLine("{0} - {1}", (DateTime.Now.Ticks - debugRender) / TimeSpan.TicksPerMillisecond, redrawGraphs ? "1" : "0");

            redrawGraphs = false;
        }

        private void btClearLive_Click(object sender, EventArgs e)
        {
            liveBode.Clear();
            liveBode.redraw = true;
            Invalidate();
        }

        private void importFile(string filename)
        {
            if (imported.Count > 4) return;
            BodeList l = new BodeList(filename);
            imported.Add(l);
            ToolStripItem removeTemp = new ToolStripMenuItem();
            removeTemp.Text = l.name;
            removeTemp.Click += removeTemp_Click;
            removeImported.DropDownItems.Add(removeTemp);
            Invalidate();
        }

        void removeTemp_Click(object sender, EventArgs e)
        {
            ToolStripMenuItem menuitem = (ToolStripMenuItem)sender;

            for (int i = 0; i < imported.Count; i++)
            {
                if (imported[i].name == menuitem.Text)
                {
                    imported.RemoveAt(i);
                    removeImported.DropDownItems.Remove(menuitem);
                    Invalidate();
                    return;
                }
            }
        }

        private void btExportLive_Click(object sender, EventArgs e)
        {
            SaveFileDialog file = new SaveFileDialog();
            file.Filter = "Bode|*.bode";
            file.ShowDialog();
            if (file.FileName == "") return;
            Console.WriteLine(file.FileName);

            liveBode.export(file.FileName);
            liveBode.Clear();
            importFile(file.FileName);
        }

        private void tryFitToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (imported.Count == 0) return;

            BodeList l = imported[0];

            double[] xx = new double[l.Count];
            double[] yG = new double[l.Count];
            double[] yP = new double[l.Count];
            double[] w = new double[l.Count];
            int i = 0;
            foreach (BodeMeasurement m in l)
            {
                xx[i] = m.freq;
                yG[i] = m.getValue(displayType);
                yP[i] = m.getValue(displayType+3);
                if (m.freq < 1) w[i] = 10;
                else if (m.freq < 20) w[i] = 100;
                else w[i] = 5;
                i++;
            }

            double[] rG = Fit.PolynomialWeighted(xx, yG, w, 10);
            double[] rP = Fit.PolynomialWeighted(xx, yP, w, 10);

            i = 0;
            //liveBode.Clear();

            float x = 0.1f;
            while (x < 100)
            {
                float y1 = 0, y2 = 0;
                for (i = 0; i < rG.Length;i++)
                    y1 += (float)(rG[i] * Math.Pow(x, i));
                for (i = 0; i < rP.Length; i++)
                    y2 += (float)(rP[i] * Math.Pow(x, i));

                BodeMeasurement b = null;
                foreach (BodeMeasurement find in liveBode)
                {
                    if (Math.Abs(find.freq - x) < 0.01)
                    {
                        b = find;
                        b.hackSetGainPhase(displayType, y1, y2);
                        break;
                    }
                }
                if (b == null)
                {
                    b = new BodeMeasurement();
                    b.freq = x;
                    b.hackSetGainPhase(displayType, y1, y2);
                    liveBode.insertMeasurement(b);
                }
                if (x < 10) x += 0.1f;
                else if (x < 100) x += 0.5f;
            }
            liveBode.unwrap();
            liveBode.redraw = true;
            Invalidate();

            //foreach (double rr in r)
            //{
              //  Console.Write(rr + (i == 0 ? "" : (i == 1 ? "*x " : "*x^" + i)) + " + ");
                //i++;
            //}
            //Console.WriteLine("");
        }

        int MX = 0;
        int MY = 0;

        private void BodePlotter_MouseMove(object sender, MouseEventArgs e)
        {
            MX = e.X;
            MY = e.Y;
            Invalidate();
        }

        private void BodePlotter_MouseClick(object sender, MouseEventArgs e)
        {
            if (e.Button == System.Windows.Forms.MouseButtons.Right)
                contextMenu.Show(Cursor.Position);
        }

        private void openFileToolStripMenuItem_Click(object sender, EventArgs e)
        {
            OpenFileDialog file = new OpenFileDialog();
            file.Filter = "Bode|*.bode";
            //file.InitialDirectory = Properties.Settings.Default.ESCFirmware;
            file.ShowDialog();
            if (file.FileName == "") return;

            importFile(file.FileName);
            //tryFitToolStripMenuItem_Click(null, null);

            StringCollection recent = Properties.Settings.Default.ESCBodeFiles;
            if (recent == null) recent = new StringCollection();

            foreach (String s in recent)
            {
                if (s == file.FileName) return;
            }

            while (recent.Count > 9) recent.RemoveAt(9);
            recent.Insert(0, file.FileName);

            Properties.Settings.Default.ESCBodeFiles = recent;
            Properties.Settings.Default.Save();

            refreshFiles();
        }

        private void refreshFiles()
        {
            StringCollection recent = Properties.Settings.Default.ESCBodeFiles;
            while (btImport.DropDownItems.Count > 2)
                btImport.DropDownItems.RemoveAt(2);
            if (recent == null) return;

            foreach (String s in recent)
            {
                ToolStripMenuItem newItem = new ToolStripMenuItem();
                newItem.Text = s.Substring(s.LastIndexOf("\\")+1);
                newItem.Click += newItem_Click;
                newItem.Tag = s;
                btImport.DropDownItems.Add(newItem);
            }
        }

        private void saveLoaded()
        {
            StringCollection c = new StringCollection();
            foreach (BodeList l in imported)
            {
                c.Add(l.filename);
            }
            Properties.Settings.Default.ESCBodeLoaded = c;
            Properties.Settings.Default.Save();
        }

        void newItem_Click(object sender, EventArgs e)
        {
            ToolStripMenuItem item = (ToolStripMenuItem)sender;
            String f = (String)item.Tag;
            importFile(f);
        }


        public void insertMeasurement(BodeMeasurement b)
        {
            liveBode.insertMeasurement(b);
            saveLive();
        }

        public void saveLive()
        {
            StringCollection c = new StringCollection();

            foreach (BodeMeasurement m in liveBode)
            {
                c.Add(m.getLine());
            }

            Properties.Settings.Default.ESCBodeLive = c;
            Properties.Settings.Default.Save();
        }

        private void zoomOutToolStripMenuItem_Click(object sender, EventArgs e)
        {
            fromLog = 0.1f;
            toLog = 100f;
            redrawGraphs = true;
            Invalidate();
        }

        private void openloopToolStripMenuItem_Click_1(object sender, EventArgs e)
        {
            displayType = 0;
            redrawGraphs = true;
            Invalidate();
        }

        private void closedloopToolStripMenuItem_Click_1(object sender, EventArgs e)
        {
            displayType = 1;
            redrawGraphs = true;
            Invalidate();
        }

        private void plantToolStripMenuItem_Click_1(object sender, EventArgs e)
        {
            displayType = 2;
            redrawGraphs = true;
            Invalidate();
        }
    }
}
